package com.hentie.mathlib;

public class Matrix {
	
	float[] mf = new float[16];
	
	//Create Identity
	public Matrix()
	{
		mf[0]= 1;
		mf[5] =1;
		mf[10] = 1;
		mf[15] = 1;
	}
	
	private Matrix(float[] mf)
	{
		this.mf = mf;
	}
	
	public Vector3 Transform(Vector3 vec)
	{
		float x = vec.X();
		float y = vec.Y();
		float z = vec.Z();
		
		float newX = x*mf[0] + y*mf[4] + z*mf[8] + 1.0f*mf[12];
		float newY = x*mf[1] + y*mf[5] + z*mf[9] + 1.0f*mf[13];
		float newZ = x*mf[2] + y*mf[6] + z*mf[10] + 1.0f*mf[14];
		
		return new Vector3(newX,newY,newZ);
	}

	public float[] Transform(float[] vec)
	{
		float[] newVec = new float[vec.length];
		int length = vec.length/3;
		
		for(int i = 0; i < length; i++)
		{
		
		float x = vec[i*3];
		float y = vec[i*3+1];
		float z = vec[i*3 + 2];
		
		float newX = x*mf[0] + y*mf[4] + z*mf[8] + 1.0f*mf[12];
		float newY = x*mf[1] + y*mf[5] + z*mf[9] + 1.0f*mf[13];
		float newZ = x*mf[2] + y*mf[6] + z*mf[10] + 1.0f*mf[14];
		
		newVec[i*3] = newX;
		newVec[i*3+1] = newY;
		newVec[i*3 + 2] = newZ;
		
		}
		return newVec;
		
	}	
	
	//Return an identity matrix 
	public static Matrix Identity()
	{
		return new Matrix();
	}
	
	public static Matrix CreateTranslation(float x, float y, float z)
	{
		Matrix trans = new Matrix();
		trans.mf[12] = x;
		trans.mf[13] = y;
		trans.mf[14] = z;
		return trans;
	}
	
	public static Matrix CreateTranslation(Vector3 vec)
	{
		return Matrix.CreateTranslation(vec.X(),vec.Y(),vec.Z());
	}
	public static Matrix CreateScale(float x, float y, float z)
	{
		Matrix trans = new Matrix();
		trans.mf[0] = x;
		trans.mf[5] = y;
		trans.mf[10] = z;
		return trans;
	}
	
	public static Matrix CreateScale(Vector3 vec)
	{
		return Matrix.CreateScale(vec.X(),vec.Y(),vec.Z());
	}
	public static Matrix Create2DRotation(float angle)
	{
		Matrix rot = new Matrix();
		
		float cosAng = (float) Math.cos(angle);
		float sinAng = (float) Math.sin(angle);
		
		rot.mf[0] = cosAng;
		rot.mf[1] = -sinAng;
		rot.mf[4] = sinAng;
		rot.mf[5] = cosAng;
		
		return rot;
	}
	
	
	
	public static Matrix Multiply(Matrix A, Matrix B)
	{
		Matrix C = new Matrix();
		//THIS IS GONNA BE LONG
		C.mf[0]=A.mf[0]*B.mf[0] + A.mf[1]*B.mf[4] + A.mf[2]*B.mf[8] + A.mf[3]*B.mf[12];
		C.mf[1]=A.mf[0]*B.mf[1] + A.mf[1]*B.mf[5] + A.mf[2]*B.mf[9] + A.mf[3]*B.mf[13];
		C.mf[2]=A.mf[0]*B.mf[2] + A.mf[1]*B.mf[6] + A.mf[2]*B.mf[10] + A.mf[3]*B.mf[14];
		C.mf[3]=A.mf[0]*B.mf[3] + A.mf[1]*B.mf[7] + A.mf[2]*B.mf[11] + A.mf[3]*B.mf[15];
		
		C.mf[4]=A.mf[4]*B.mf[0] + A.mf[5]*B.mf[4] + A.mf[6]*B.mf[8] + A.mf[7]*B.mf[12];
		C.mf[5]=A.mf[4]*B.mf[1] + A.mf[5]*B.mf[5] + A.mf[6]*B.mf[9] + A.mf[7]*B.mf[13];
		C.mf[6]=A.mf[4]*B.mf[2] + A.mf[5]*B.mf[6] + A.mf[6]*B.mf[10] + A.mf[7]*B.mf[14];
		C.mf[7]=A.mf[4]*B.mf[3] + A.mf[5]*B.mf[7] + A.mf[6]*B.mf[11] + A.mf[7]*B.mf[15];

		C.mf[8]=A.mf[8]*B.mf[0] + A.mf[9]*B.mf[4] + A.mf[10]*B.mf[8] + A.mf[11]*B.mf[12];
		C.mf[9]=A.mf[8]*B.mf[1] + A.mf[9]*B.mf[5] + A.mf[10]*B.mf[9] + A.mf[11]*B.mf[13];
		C.mf[10]=A.mf[8]*B.mf[2] + A.mf[9]*B.mf[6] + A.mf[10]*B.mf[10] + A.mf[11]*B.mf[14];
		C.mf[11]=A.mf[8]*B.mf[3] + A.mf[9]*B.mf[7] + A.mf[10]*B.mf[11] + A.mf[11]*B.mf[15];

		C.mf[12]=A.mf[12]*B.mf[0] + A.mf[13]*B.mf[4] + A.mf[14]*B.mf[8] + A.mf[15]*B.mf[12];
		C.mf[13]=A.mf[12]*B.mf[1] + A.mf[13]*B.mf[5] + A.mf[14]*B.mf[9] + A.mf[15]*B.mf[13];
		C.mf[14]=A.mf[12]*B.mf[2] + A.mf[13]*B.mf[6] + A.mf[14]*B.mf[10] + A.mf[15]*B.mf[14];
		C.mf[15]=A.mf[12]*B.mf[3] + A.mf[13]*B.mf[7] + A.mf[14]*B.mf[11] + A.mf[15]*B.mf[15];
		
		return C;
		
		
	}
	
}
